PMSM_Param.R = 2.875;
PMSM_Param.Ld = 0.00153;
PMSM_Param.Lq = PMSM_Param.Ld;
PMSM_Param.lambda = 0.175;
PMSM_Param.p = 4;
PMSM_Param.J = 0.8e-3;
PMSM_Param.F = 0;
PMSM_Param.Tf = 0;

Tm = 3; % shaft mechanical torque
Upp = 203.4; % phase-to-phase voltage (rms)
Vdc = 400; % DC-link voltage
Fbase = 200/3; % fundamental frequency
Fsw = 4e3; % switching frequency
Ts = 1e-6; % sample time
stop_time = 0.15;
time = 0:Ts:stop_time;
Ua_sin = (Upp*sqrt(2/3)) .* sin(2.*pi.*Fbase.*time);
Ub_sin = (Upp*sqrt(2/3)) .* sin(2.*pi.*Fbase.*time - 2*pi/3);
Uc_sin = (Upp*sqrt(2/3)) .* sin(2.*pi.*Fbase.*time - 4*pi/3);

open_system('sim/PMSM_Simulation_Model');
tic; sim('sim/PMSM_Simulation_Model'); toc;
Ua_pwm = Upwm_Input{1}.Values.Data(:, 1)';
Ub_pwm = Upwm_Input{1}.Values.Data(:, 2)';
Uc_pwm = Upwm_Input{1}.Values.Data(:, 3)';

% 用矩形PWM波作为输入激励可以求解得到带纹波的电流响应，但是需要将绝对误差设置得小一些，比如1e-4
% 与正弦波形相比，矩形PWM波作为输入时的计算时间大幅提升，目前尚不清楚为何Simulink的计算速度反而更快
tic
tspan = [0 0.15];
y0 = [0 0 0 0];
opts = odeset('RelTol', 1e-4);
[t, y] = ode45(@(t, y) pmsm_ode(t, y, Tm, Ua_sin, Ub_sin, Uc_sin, time, PMSM_Param), ...
    tspan, y0, opts);
toc
theta_e = interp1(t', y(:, 1)', time);
theta_e = PMSM_Param.p .* theta_e;
id = interp1(t', y(:, 3)', time);
iq = interp1(t', y(:, 4)', time);
Ia = cos(theta_e) .* id - sin(theta_e) .* iq;
Ib = cos(theta_e-2*pi/3) .* id - sin(theta_e-2*pi/3) .* iq;
Ic = cos(theta_e-4*pi/3) .* id - sin(theta_e-4*pi/3) .* iq;
figure
tiledlayout(3, 1);
nexttile
plot(Sinusoidal_Input{2}.Values.Time, Sinusoidal_Input{2}.Values.Data(:, 1));
hold on
plot(time, Ia);
nexttile
plot(Sinusoidal_Input{2}.Values.Time, Sinusoidal_Input{2}.Values.Data(:, 2));
hold on
plot(time, Ib);
nexttile
plot(Sinusoidal_Input{2}.Values.Time, Sinusoidal_Input{2}.Values.Data(:, 3));
hold on
plot(time, Ic);

% 求解稳态运行状态
Upeak = Upp * sqrt(2/3);
Uangle = 0;
syms theta_e id iq
eqns = [1.5*PMSM_Param.p * (PMSM_Param.lambda*iq + (PMSM_Param.Ld-PMSM_Param.Lq)*id*iq) ...
    == PMSM_Param.Tf + PMSM_Param.F*Fbase*2*pi/PMSM_Param.p + Tm, ...
    Upeak*sin(Uangle-theta_e) - PMSM_Param.R*id + PMSM_Param.Lq*Fbase*2*pi*iq == 0, ...
    -Upeak*cos(Uangle-theta_e) - PMSM_Param.R*iq - PMSM_Param.Ld*Fbase*2*pi*id - PMSM_Param.lambda*Fbase*2*pi == 0];
S = solve(eqns, [theta_e, id, iq]);
index = find(double(S.theta_e) < Uangle, 1);
theta_e = double(S.theta_e(index));
% id = double(vpa(S.id(index))); iq = double(vpa(S.iq(index)));
omega_e = Fbase * 2 * pi;
% ud = Upeak*sin(Uangle-theta_e); % 稳态下d-axis voltage，其中不含谐波分量
% uq = -Upeak*cos(Uangle-theta_e); % 稳态下q-axis voltage，其中不含谐波分量
ud = (2/3) .* (cos(omega_e.*time+theta_e) .* Ua_pwm ...
    + cos(omega_e.*time+theta_e-2*pi/3) .* Ub_pwm ...
    + cos(omega_e.*time+theta_e-4*pi/3) .* Uc_pwm);
uq = (2/3) .* (-sin(omega_e.*time+theta_e) .* Ua_pwm ...
    - sin(omega_e.*time+theta_e-2*pi/3) .* Ub_pwm ...
    - sin(omega_e.*time+theta_e-4*pi/3) .* Uc_pwm);
% % 变步长ode45求解器求解
% % 在相同精度下，pmsm_ode2模型计算速度略比pmsm_ode模型快一些，总体来说还是太慢了
% % 在矩形PWM波输入，且精度为1e-4时计算时间超过350s，但是波形仍然不准确
% tic
% tspan = [0 0.15];
% y0 = [0 0];
% opts = odeset('RelTol', 1e-4);
% [t, y] = ode45(@(t, y) pmsm_ode2(t, y, ud, uq, Fbase*2*pi, time, PMSM_Param), ...
%     tspan, y0, opts);
% toc
% id = interp1(t', y(:, 1)', time);
% iq = interp1(t', y(:, 2)', time);
% Ia = cos(Fbase*2*pi.*time+theta_e) .* id - sin(Fbase*2*pi.*time+theta_e) .* iq;
% Ib = cos(Fbase*2*pi.*time+theta_e-2*pi/3) .* id - sin(Fbase*2*pi.*time+theta_e-2*pi/3) .* iq;
% Ic = cos(Fbase*2*pi.*time+theta_e-4*pi/3) .* id - sin(Fbase*2*pi.*time+theta_e-4*pi/3) .* iq;
% figure
% tiledlayout(3, 1);
% nexttile
% plot(Upwm_Input{2}.Values.Time, Upwm_Input{2}.Values.Data(:, 1));
% hold on
% plot(time, Ia);
% nexttile
% plot(Upwm_Input{2}.Values.Time, Upwm_Input{2}.Values.Data(:, 2));
% hold on
% plot(time, Ib);
% nexttile
% plot(Upwm_Input{2}.Values.Time, Upwm_Input{2}.Values.Data(:, 3));
% hold on
% plot(time, Ic);
% 状态空间方程求解
tic
A = [-PMSM_Param.R/PMSM_Param.Ld,         omega_e*PMSM_Param.Lq/PMSM_Param.Ld; ...
    -omega_e*PMSM_Param.Ld/PMSM_Param.Lq, -PMSM_Param.R/PMSM_Param.Lq];
B = [1/PMSM_Param.Ld, 0, 0; 0, 1/PMSM_Param.Lq, -omega_e/PMSM_Param.Lq];
C = [1, 0; 0, 1];
D = [0, 0, 0; 0, 0, 0];
Z = ss(A, B, C, D);
init_state = zeros(1, size(C, 2));
fundamental_time = 0:Ts:1/Fbase;
ud = ud(1:1/Fbase/Ts+1); uq = uq(1:1/Fbase/Ts+1);
while 1
    current = lsim(Z, [ud; uq; PMSM_Param.lambda.*ones(size(fundamental_time))], ...
        fundamental_time, init_state);
    init_state = current(end, :);
    if abs(current(end, :) - current(1, :)) < 1e-5
        break
    end
end
toc
id = current(:, 1)'; iq = current(:, 2)';
Ia = cos(Fbase*2*pi.*fundamental_time+theta_e) .* id ...
    - sin(Fbase*2*pi.*fundamental_time+theta_e) .* iq;
Ib = cos(Fbase*2*pi.*fundamental_time+theta_e-2*pi/3) .* id ...
    - sin(Fbase*2*pi.*fundamental_time+theta_e-2*pi/3) .* iq;
Ic = cos(Fbase*2*pi.*fundamental_time+theta_e-4*pi/3) .* id ...
    - sin(Fbase*2*pi.*fundamental_time+theta_e-4*pi/3) .* iq;
figure
tiledlayout(3, 1);
nexttile
plot(Upwm_Input{2}.Values.Time, Upwm_Input{2}.Values.Data(:, 1));
hold on
plot(time, [repmat(Ia(1:end-1), 1, stop_time*Fbase), Ia(end)]);
nexttile
plot(Upwm_Input{2}.Values.Time, Upwm_Input{2}.Values.Data(:, 2));
hold on
plot(time, [repmat(Ib(1:end-1), 1, stop_time*Fbase), Ib(end)]);
nexttile
plot(Upwm_Input{2}.Values.Time, Upwm_Input{2}.Values.Data(:, 3));
hold on
plot(time, [repmat(Ic(1:end-1), 1, stop_time*Fbase), Ic(end)]);


% y(1): theta, rotor angular position; y(2): omega_m, angular velocity of rotor;
% y(3): id, d-axis current; y(4): iq, q-axis current.
function dydt = pmsm_ode(t, y, tm, ua, ub, uc, time, param)
interp_ua = interp1(time, ua, t);
interp_ub = interp1(time, ub, t);
interp_uc = interp1(time, uc, t);
Te = 1.5 .* param.p .* (y(4) .* param.lambda + (param.Ld - param.Lq) .* y(3) .* y(4));
theta_e = param.p .* y(1);
ud = 2/3 .* (cos(theta_e) .* interp_ua + cos(theta_e-2*pi/3) .* interp_ub ...
    + cos(theta_e+2*pi/3) .* interp_uc);
uq = 2/3 .* (-sin(theta_e).*interp_ua - sin(theta_e-2*pi/3).*interp_ub ...
    - sin(theta_e+2*pi/3).*interp_uc);
dydt = zeros(4, 1);
dydt(1) = y(2);
dydt(2) = (Te - tm - param.Tf - param.F .* y(2)) ./ param.J;
dydt(3) = -param.R ./ param.Ld .* y(3) + param.Lq ./ param.Ld .* param.p .* y(2) .* y(4) ...
    + ud ./ param.Ld;
dydt(4) = -param.R ./ param.Lq .* y(4) - param.Ld ./ param.Lq .* param.p .* y(2) .* y(3) ...
    - param.p .* y(2) .* param.lambda ./ param.Lq + uq ./ param.Lq;
end

% function dydt = pmsm_ode2(t, y, ud, uq, omega_e, time, param)
% interp_ud = interp1(time, ud, t);
% interp_uq = interp1(time, uq, t);
% dydt = zeros(2, 1);
% dydt(1) = -param.R ./ param.Ld .* y(1) + param.Lq ./ param.Ld .* omega_e .* y(2) ...
%     + interp_ud ./ param.Ld;
% dydt(2) = -param.R ./ param.Lq .* y(2) - param.Ld ./ param.Lq .* omega_e .* y(1) ...
%     - omega_e .* param.lambda ./ param.Lq + interp_uq ./ param.Lq;
% end
